#include "proto.hpp"

#include <commons/base/endian.hpp>
#include <commons/util/crc.hpp>
#include <commons/util/util.hpp>

#include <cstring>

const static uint16_t HEADER_FLAG = 0xA0AB;
const static uint8_t TAIL_FLAG = 0xCD;

namespace sros::device
{
    proto &proto::ins()
    {
        static proto prot;
        return prot;
    }

    std::vector<uint8_t> proto::pck_version()
    {
        return pck_empty(Cmd::Version);
    }

    std::vector<uint8_t> proto::pck_heartbeat()
    {
        return pck_empty(Cmd::Heartbeat);
    }

    std::vector<uint8_t> proto::pck_reset()
    {
        return pck_empty(Cmd::Reset);
    }

    std::vector<uint8_t> proto::pck_init_board(const CanConfig &t)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Init);
        endian.write<uint16_t>(sizeof(CanConfig));

        endian.write<uint8_t>(t.type);
        endian.write<uint16_t>(t.index);
        endian.write<uint16_t>(t.prescaler);
        endian.write<uint8_t>(t.seg1);
        endian.write<uint8_t>(t.seg2);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_init_board(const UartConfig &t)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Init);
        endian.write<uint16_t>(sizeof(UartConfig));

        endian.write<uint8_t>(t.type);
        endian.write<uint16_t>(t.index);
        endian.write<uint32_t>(t.bitrate);
        endian.write<uint8_t>(t.wordlength);
        endian.write<uint8_t>(t.stopbit);
        endian.write<uint8_t>(t.parity);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_init_board(const InputIOConfig &t)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Init);
        endian.write<uint16_t>(sizeof(InputIOConfig));

        endian.write<uint8_t>(t.type);
        endian.write<uint8_t>(t.port);
        endian.write<uint8_t>(t.pin);
        endian.write<uint8_t>(t.mode);
        endian.write<uint8_t>(t.pull_up_down);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_init_board(const OutputIOConfig &t)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Init);
        endian.write<uint16_t>(sizeof(OutputIOConfig));

        endian.write<uint8_t>(t.type);
        endian.write<uint8_t>(t.port);
        endian.write<uint8_t>(t.pin);
        endian.write<uint8_t>(t.mode);
        endian.write<uint8_t>(t.pull_up_down);
        endian.write<uint8_t>(t.out_level);
        endian.write<uint8_t>(t.spd);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_empty(const Cmd &cmd)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(cmd);
        endian.write<uint16_t>(0);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_trans(const ReqCanMsg &msg)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Trans);
        endian.write<uint16_t>(sizeof(ReqCanMsg) + 1);
        endian.write<uint8_t>(ConfigType::CAN);
        endian.write<uint16_t>(msg.index);

        endian.write<uint32_t>(msg.can_msg.Header.StdId);
        endian.write<uint32_t>(msg.can_msg.Header.ExtId);
        endian.write<uint32_t>(msg.can_msg.Header.IDE);
        endian.write<uint32_t>(msg.can_msg.Header.RTR);
        endian.write<uint32_t>(msg.can_msg.Header.DLC);
        endian.write<uint32_t>(msg.can_msg.Header.TransmitGlobalTime);

        for (int i = 0; i < 8; i++)
        {
            endian.write<uint8_t>(msg.can_msg.Data[i]);
        }

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_trans(const ReqUartMsg &msg)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Trans);
        endian.write<uint16_t>(msg.data.size() + 3);
        endian.write<uint8_t>(ConfigType::UART);
        endian.write<uint16_t>(msg.index);
        endian.write_array(msg.data);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_trans(const ReqIoMsg &msg)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Trans);
        endian.write<uint16_t>(4);
        endian.write<uint8_t>(ConfigType::OUTPUTIO);
        endian.write<uint8_t>(msg.port);
        endian.write<uint8_t>(msg.pin);
        endian.write<uint8_t>(msg.lvl);

        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_power(const uint8_t &id, const uint8_t &is_on)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Power);
        endian.write<uint16_t>(2);
        endian.write<uint8_t>(id);
        endian.write<uint8_t>(is_on);
        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    std::vector<uint8_t> proto::pck_encoder(const uint8_t &id, const int32_t &pulse)
    {
        base::WriteEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian;
        endian.write<uint16_t>(HEADER_FLAG);
        endian.write<uint8_t>(Cmd::Encoder);
        endian.write<uint16_t>(5);
        endian.write<uint8_t>(id);
        endian.write<int32_t>(pulse);
        uint16_t crc = crc::crc16_ccitt(endian.buffer());
        endian.write<uint16_t>(crc);
        endian.write<uint8_t>(TAIL_FLAG);

        return endian.buffer();
    }

    RetBool proto::unpack(const std::vector<uint8_t> &payload)
    {
        uint8_t cmd = 0;
        std::vector<uint8_t> buffer;

        auto ret = unpack_msg(payload, cmd, buffer);
        if (!ret.v)
        {
            return ret;
        }
        ret = unpack_dispatch(cmd, buffer);
        return ret;
    }

    RetBool proto::unpack_version_msg(const std::vector<uint8_t> &payload, RspVersionMsg &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t first_v;
        auto ret = endian.read<uint8_t>(first_v);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t second_v;
        ret = endian.read<uint8_t>(second_v);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t third_v;
        ret = endian.read<uint8_t>(third_v);
        if (!ret.v)
        {
            return ret;
        }

        uint16_t year;
        ret = endian.read<uint16_t>(year);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t month;
        ret = endian.read<uint8_t>(month);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t day;
        ret = endian.read<uint8_t>(day);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t hour;
        ret = endian.read<uint8_t>(hour);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t minute;
        ret = endian.read<uint8_t>(minute);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t second;
        ret = endian.read<uint8_t>(second);
        if (!ret.v)
        {
            return ret;
        }

        out.version = util::format("v%d.%d.%d", first_v, second_v, third_v);
        out.datetime = util::format("%d-%d-%d %d:%d:%d", year, month, day, hour, minute, second);

        return {true};
    }

    RetBool proto::unpack_can_msg(const std::vector<uint8_t> &payload, RspCanMsg &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);
        uint8_t type = 0;
        auto ret = endian.read<uint8_t>(type);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint16_t>(out.index);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.StdId);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.ExtId);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.IDE);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.RTR);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.DLC);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.Timestamp);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint32_t>(out.msg.Header.FilterMatchIndex);
        if (!ret.v)
        {
            return ret;
        }

        std::vector<uint8_t> data;
        ret = endian.read_array<uint8_t>(data, 8);
        if (!ret.v)
        {
            return ret;
        }

        memcpy(out.msg.Data, data.data(), 8);
        return ret;
    }

    RetBool proto::unpack_uart_msg(const std::vector<uint8_t> &payload, RspUartMsg &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t type;
        auto ret = endian.read<uint8_t>(type);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read<uint16_t>(out.index);
        if (!ret.v)
        {
            return ret;
        }

        auto sz = payload.size() - 3;
        ret = endian.read_array(out.buffer, sz);
        if (!ret.v)
        {
            return ret;
        }

        return ret;
    }

    RetBool proto::unpack_io_msg(const std::vector<uint8_t> &payload, RspIoMsg &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t type;
        auto ret = endian.read<uint8_t>(type);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t c_port;
        ret = endian.read<uint8_t>(c_port);
        if (!ret.v)
        {
            return ret;
        }
        out.port = static_cast<hal::IOPort>(c_port);

        uint8_t c_pin;
        ret = endian.read<uint8_t>(c_pin);
        if (!ret.v)
        {
            return ret;
        }
        out.pin = c_pin;

        uint8_t c_lvl;
        ret = endian.read<uint8_t>(c_lvl);
        if (!ret.v)
        {
            return ret;
        }
        out.lvl = static_cast<hal::IOOutLevel>(c_lvl);

        return ret;
    }

    RetBool proto::unpack_msg(const std::vector<uint8_t> &payload, uint8_t &cmd, std::vector<uint8_t> &buffer)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);
        
        uint16_t head_flag = 0;
        auto ret = endian.read(head_flag);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read(cmd);
        if (!ret.v)
        {
            return ret;
        }

        uint16_t buffer_size = 0;
        ret = endian.read(buffer_size);
        if (!ret.v)
        {
            return ret;
        }

        ret = endian.read_array(buffer, buffer_size);
        if (!ret.v)
        {
            return ret;
        }

        uint16_t crc = 0;
        ret = endian.read(crc);
        if (!ret.v)
        {
            return ret;
        }

        uint8_t tail_flag = 0;
        ret = endian.read(tail_flag);
        if (!ret.v)
        {
            return ret;
        }

        if (head_flag != HEADER_FLAG)
        {
            return {false, "head flag is not equal"};
        }

        if (tail_flag != TAIL_FLAG)
        {
            return {false, "tail flag is not equal"};
        }

        std::vector<uint8_t> crc_payload(payload.begin(), payload.begin() + payload.size() - 3);
        uint16_t cal_crc = crc::crc16_ccitt(crc_payload);
        if (cal_crc != crc)
        {
            std::string msg = util::format("cal_crc: %d is not equal crc:%d\n", cal_crc, crc);
            return {false, msg};
        }

        return {true};
    }

    RetBool proto::unpack_init_msg(const std::vector<uint8_t> &payload, RspInitMsg &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t c_type = 0;
        auto ret = endian.read<uint8_t>(c_type);
        if (!ret.v)
        {
            return ret;
        }
        out.type = static_cast<ConfigType>(c_type);

        uint16_t c_index = 0;
        ret = endian.read<uint16_t>(c_index);
        if (!ret.v)
        {
            return ret;
        }
        out.index = c_index;

        uint8_t c_ec = 0;
        ret = endian.read<uint8_t>(c_ec);
        if (!ret.v)
        {
            return ret;
        }
        out.ec = c_ec;

        out.msg = std::string(payload.begin() + 4, payload.end());

        return {true};
    }

    RetBool proto::unpack_can_state(const std::vector<uint8_t> &payload, RspCanState &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t type = 0;
        auto ret = endian.read<uint8_t>(type);
        if (!ret.v)
        {
            return ret;
        }

        uint16_t index = 0;
        ret = endian.read<uint16_t>(index);
        if (!ret.v)
        {
            return ret;
        }
        out.index = index;

        uint8_t state = 0;
        ret = endian.read<uint8_t>(state);
        if (!ret.v)
        {
            return ret;
        }
        out.state = static_cast<hal::CANState>(state);

        uint32_t error = 0;
        ret = endian.read<uint32_t>(error);
        if (!ret.v)
        {
            return ret;
        }
        out.error = error;

        uint8_t active = 0;
        ret = endian.read<uint8_t>(active);
        if (!ret.v)
        {
            return ret;
        }
        out.active  = active;

        return {true};
    }

    RetBool proto::unpack_uart_state(const std::vector<uint8_t> &payload, RspUartState &out)
    {
        base::ReadEndian<base::ENDIAN_TYPE::BIG_ENDIAN_> endian(payload);

        uint8_t type = 0;
        auto ret = endian.read<uint8_t>(type);
        if (!ret.v)
        {
            return ret;
        }

        uint16_t index = 0;
        ret = endian.read<uint16_t>(index);
        if (!ret.v)
        {
            return ret;
        }
        out.index = index;

        uint8_t state = 0;
        ret = endian.read<uint8_t>(state);
        if (!ret.v)
        {
            return ret;
        }
        out.state = static_cast<hal::UartState>(state);

        uint32_t error = 0;
        ret = endian.read<uint32_t>(error);
        if (!ret.v)
        {
            return ret;
        }
        out.error = error;

        uint8_t active = 0;
        ret = endian.read<uint8_t>(active);
        if (!ret.v)
        {
            return ret;
        }
        out.active  = active;

        return {true};
    }

    RetBool proto::unpack_power_state(const std::vector<uint8_t> &payload, std::map<uint32_t, bool> &out)
    {
        if (payload.empty())
        {
            return {false, "power state payload size is zero"};
        }
        if (payload.size() % 2 != 0)
        {
            return {false, "power state payload size is not even"};
        }

        out.clear();

        uint16_t size = payload.size() / 2;
        for(int i = 0; i < size; i++)
        {
            int index = i * 2;
            uint8_t id = payload.at(index);
            uint8_t is_on = payload.at(index + 1);
            out[id] = is_on == 1;
        }

        return {true};
    }

    RetBool proto::unpck_encoder_msg(const std::vector<uint8_t> &payload, std::map<uint32_t, int32_t> &out)
    {
        if (payload.empty())
        {
            return {false, "encoder msg payload size is zero"};
        }
        if (payload.size() % 5 != 0)
        {
            return {false, "encoder msg payload size is not multiples of five"};
        }

        out.clear();

        uint16_t size = payload.size() / 5;
        for(int i = 0; i < size; i++)
        {
            int index = i * 5;
            uint8_t id = payload.at(index);
            int32_t pluse = ((int32_t)payload.at(index + 1) << 24) |
                            ((int32_t)payload.at(index + 2) << 16) |
                            ((int32_t)payload.at(index + 3) << 8)  |
                            ((int32_t)payload.at(index + 4));
            out[id] = pluse;
        }

        return {true};
    }

    RetBool proto::unpack_dispatch(const uint8_t &cmd, const std::vector<uint8_t> &payload)
    {
        switch ((Cmd)cmd)
        {
        case Cmd::Version:
        {
            RspVersionMsg msg;
            auto ret = unpack_version_msg(payload, msg);
            if (ret.v && version_cb_)
            {
                version_cb_(msg);
            }
            return ret;
        }break;
        case Cmd::Init:
        {
            RspInitMsg msg;
            auto ret = unpack_init_msg(payload, msg);
            if (ret.v && init_cb_)
            {
                init_cb_(msg);
            }
            return ret;
        }break;
        case Cmd::Reset:
        {
            return {true};
        }break;
        case Cmd::Trans:
        {
            uint8_t type = 0;
            try
            {
                type = payload.at(0);
            }
            catch (const std::exception &e)
            {
                return {false, e.what()};
            }

            switch (ConfigType(type))
            {
            case ConfigType::CAN:
            {
                RspCanMsg msg;
                auto ret = unpack_can_msg(payload, msg);
                if (ret.v && can_cb_)
                {
                    can_cb_(msg);
                }
                return ret;
            }break;
            case ConfigType::UART:
            {
                RspUartMsg msg;
                auto ret = unpack_uart_msg(payload, msg);
                if (ret.v && uart_cb_)
                {
                    uart_cb_(msg);
                }
                return ret;
            }break;
            case ConfigType::INPUTIO:
            case ConfigType::OUTPUTIO:
            {
                RspIoMsg msg;
                auto ret = unpack_io_msg(payload, msg);
                if (ret.v && io_cb_)
                {
                    io_cb_(msg);
                }
                return ret;
            }break;
            default:
            {
                return {false, "unkown channel type."};
            }break;
            }
        }break;
        case Cmd::Status:
        {
            uint8_t type = 0;
            try
            {
                type = payload.at(0);
            }
            catch (const std::exception &e)
            {
                return {false, e.what()};
            }

            switch (ConfigType(type))
            {
            case ConfigType::CAN:
            {
                RspCanState msg;
                auto ret = unpack_can_state(payload, msg);
                if (ret.v && can_state_cb_)
                {
                    can_state_cb_(msg);
                }
                return ret;
            }break;
            case ConfigType::UART:
            {
                RspUartState msg;
                auto ret = unpack_uart_state(payload, msg);
                if (ret.v && uart_state_cb_)
                {
                    uart_state_cb_(msg);
                }
                return ret;
            }break;
            default:
            {
                return {false, "unkown channel type"};
            }break;
            }
        }break;
        case Cmd::Heartbeat:
        {
            return {true};
        }break;
        case Cmd::Power:
        {
            std::map<uint32_t, bool> power_state_list;
            auto ret = unpack_power_state(payload, power_state_list);
            if (ret.v && power_state_cb_)
            {
                power_state_cb_(power_state_list);
            }
            return ret;
        }break;
        case Cmd::Encoder:
        {
            std::map<uint32_t, int32_t> encoder_msg_list;
            auto ret = unpck_encoder_msg(payload, encoder_msg_list);

            if (ret.v && encoder_cb_)
            {
                encoder_cb_(encoder_msg_list);
            }
            return ret;
        }break;
        default:
        {
            return {false, "unkown cmd"};
        }break;
        }
    }
}